#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>


// AGV高度范围
#define MIN_HEIGHT 0.0       // 最小高度
#define MAX_HEIGHT 0.2       // 最大高度
#define DEFAULT_VELOCITY 0.1 // 默认速度

class PalletPick : public rclcpp::Node
{
public:
    PalletPick() : Node("pallet_pick"), logger_(this->get_logger())
    {
        RCLCPP_INFO(logger_, "节点已经启动!");
        subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 1,
            std::bind(&PalletPick::joint_states_callback, this, std::placeholders::_1));
        publisher_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>(
            "/arm_connect_joint_controller/joint_trajectory", 1);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(5000),
                                         std::bind(&PalletPick::timer_callback, this));
    }

private:
    void timer_callback()
    {
        move_joint(target_height_);
    }
    /**
     * @brief 移动机械臂
     */
    void move_joint(double position)
    {
        joint_trajectory_msg_.joint_names = {"arm_connect_joint"};
        auto point = trajectory_msgs::msg::JointTrajectoryPoint();
        point.positions.push_back(position);
        point.velocities.push_back(DEFAULT_VELOCITY);
        // 确保时间戳严格递增
        static double last_time = 0.0;
        last_time += 1.0;
        point.time_from_start = rclcpp::Duration::from_seconds(last_time);
        joint_trajectory_msg_.points.push_back(point);
        publisher_->publish(joint_trajectory_msg_);
        RCLCPP_INFO(logger_, "Publishing: position='%f',%f", position, last_time);
    }
    /**
     * @brief Joint states callback
     */
    void joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        for (size_t i = 0; i < msg->name.size(); i++)
        {
            if (msg->name[i] == "arm_connect_joint")
            {
                RCLCPP_INFO(logger_, "当前机械臂位置为: position=%f,vel=%f", msg->position[i], msg->velocity[i]);
                arm_connect_callback(msg->position[i], msg->velocity[i]);
            }
        }
    }
    /**
     * @brief 获取机械臂连接高度
     */
    void arm_connect_callback(double height, double velocity)
    {
        if (velocity > DEFAULT_VELOCITY / 2)
        {
            RCLCPP_DEBUG_ONCE(logger_, "移动中,position=: %f,velocity=%f", height, velocity);
            return;
        }
        if (height > MAX_HEIGHT / 2)
        { // 处于上升状态
            RCLCPP_DEBUG_ONCE(logger_, "当前机械臂连接高度为: %f, 尝试下降高度", height);
            target_height_ = MIN_HEIGHT;
        }
        else if (height < MIN_HEIGHT / 2)
        { // 处于下降状态
            target_height_ = MAX_HEIGHT;
            RCLCPP_DEBUG_ONCE(logger_, "当前机械臂连接高度为: %f, 尝试上升高度", height);
        }
    }
    rclcpp::Logger logger_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscriber_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr publisher_;
    trajectory_msgs::msg::JointTrajectory joint_trajectory_msg_;
    double target_height_ = 0.0;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[])
{
    /* 初始化rclcpp  */
    rclcpp::init(argc, argv);
    /* 产生一个 节点 */
    auto node = std::make_shared<PalletPick>();
    /* 运行节点，并检测退出信号 Ctrl+C */
    rclcpp::spin(node);
    /* 停止运行 */
    rclcpp::shutdown();
    return 0;
}